# Copyright 2021 Josh Newans
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rclpy
from rclpy.node import Node

from std_msgs.msg import Header
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistWithCovariance
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseWithCovariance
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import PoseStamped
from rclpy.qos import QoSProfile, ReliabilityPolicy
class Mavros2odom(Node):

    def __init__(self):
        super().__init__('odom')#节点名称
        #偏移补偿
        self.declare_parameter("offset_x", 0.0)
        self.declare_parameter("offset_y", 0.0)
        self.declare_parameter("offset_z", 0.0)
        ####################
        self.declare_parameter("child_frame_id", "base_footprint")# 声明一个名为'child_link'的参数，默认值为空字符串，参数可用于在运行时配置节点的某些特性
        self.child_frame_id = str(self.get_parameter("child_frame_id").value)# 获取刚刚声明的'child_link'参数的值，并转换为字符串类型，赋值给self.frame_id变量
        self.declare_parameter("frame_id", "odom")# 声明一个名为'child_link'的参数，默认值为空字符串，参数可用于在运行时配置节点的某些特性
        self.frame_id = str(self.get_parameter("frame_id").value)# 获取刚刚声明的'child_link'参数的值，并转换为字符串类型，赋值给self.frame_id变量

        self.publisher_odom = self.create_publisher(Odometry, 'odom', 10)#创建发布方
        # self.timer = self.create_timer(2.0,self.on_timer)

        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )

        #创建订阅方1个
        self.mavros_local_position_odom = self.create_subscription(
            Odometry,
            'mavros/local_position/odom',
            self.listener_callback_mavros_local_position_odom,
            qos_profile)

        self.get_logger().info("等待mavros2odom...") 

    #订阅方回调函数，订阅到数据的话就执行此函数
    # def listener_callback_mavros_local_position_odom(self, inMsg):
    #     new_odom = Odometry()
    #     new_odom = inMsg
    #     new_odom.header.frame_id = self.frame_id
    #     new_odom.child_frame_id = self.child_frame_id
    #     self.publisher_odom.publish(new_odom)

    def listener_callback_mavros_local_position_odom(self, inMsg):
        # 获取当前参数值 (每次回调都获取最新参数)
        offset_x = self.get_parameter("offset_x").value
        offset_y = self.get_parameter("offset_y").value
        offset_z = self.get_parameter("offset_z").value
        new_odom = Odometry()
        
        # 复制原始消息内容
        new_odom.header = inMsg.header
        new_odom.child_frame_id = self.child_frame_id
        new_odom.pose = inMsg.pose
        new_odom.twist = inMsg.twist
        
        # 设置新的坐标系ID
        new_odom.header.frame_id = self.frame_id
        
        # 应用位置偏移补偿
        new_odom.pose.pose.position.x = inMsg.pose.pose.position.x - offset_x
        new_odom.pose.pose.position.y = inMsg.pose.pose.position.y - offset_y
        new_odom.pose.pose.position.z = 0.0
        
        # 发布补偿后的odom消息
        self.publisher_odom.publish(new_odom)


def main(args=None):
    rclpy.init(args=args)

    mavros2odom = Mavros2odom()
    rclpy.spin(mavros2odom)

    mavros2odom.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
